function obj = Doppler_position_using_EKF(obj,ini_state)
%DOPPLER_POSITION_USING_EKF 使用EKF进行多普勒定位
%   参考Instantaneous velocity determination and
% positioning using Doppler shift from a LEO constellation
%% 相关常数
T = 1;%采样率
env = enviroment;
f_c = obj.f_obs;
% c = env.c;
%% 初始化状态向量和协方差矩阵
obj.EKF(1).S = ini_state;
obj.EKF(1).P = 0.01*eye(7);
ekf_Q = 0.001*blkdiag(1*eye(3),eye(4));
%% 定义状态转移矩阵
phi = eye(7);
phi(1,4) = T;
phi(2,5) = T;
phi(3,6) = T;%状态转移矩阵(可能有误)
% tao = [[T^2/2;T^2/2;T^2/2],[0;0;0];[0;0;0;0],[T;T;T;0]]; 
%% 获取观测值
doppler_obs_array = {obj.doppler_obs.doppler}';
visible_sat = {obj.sat_obs.satobs}';
for k = 2:length(obj.doppler_obs)

    % inum = length(cell2mat(visible_sat(k)));
    %预测
    obj.EKF(k).S = (phi*obj.EKF(k-1).S')';
    obj.EKF(k).P = phi*obj.EKF(k-1).P*phi'+ekf_Q;
    % EKF(k).P = phi*EKF(k-1).P*phi';%这步发散减慢，为什么？
    %矫正
    p_s = obj.satpos_obs((k)).satpos(:,1:3);
    v_s = obj.satpos_obs((k)).satpos(:,4:6);
    obj.EKF(k).C = df_ds(p_s, obj.EKF((k-1)).S, v_s, f_c);
    G = obj.EKF(k).C;
    obj.DDOP(k-1).DGDOP = sqrt(trace(inv(G'*G)));

    obj.EKF(k).n = length(cell2mat(visible_sat(k)));
    
    % D = df_ds(p_s, [0.01,0.01,0.01,0.01,0.01,0.01,0.01], v_s, f_c);
    % D = df_ds(p_s, obj.EKF(k).S, v_s, f_c)*blkdiag(1,1,1,0,0,0,0);
    % D = diag(sqrt(sum(p_s.^2, 2))/10000);
    % D = diag(sqrt(sum(v_s.^2, 2)));
    D = 1;
    ekf_R = D*eye(obj.EKF(k).n)*D';%权值矩阵，具体待定
    % ekf_R = D*eye(7)*D';
    % ekf_R = eye(obj.EKF(k).n)/(D*D');
    obj.EKF(k).K = obj.EKF(k).P*obj.EKF(k).C'/(obj.EKF(k).C*obj.EKF(k).P*obj.EKF(k).C'+ekf_R);
    obj.EKF(k).S = obj.EKF(k).S+(obj.EKF(k).K*(cell2mat(doppler_obs_array(k))-fd(p_s, obj.EKF(k).S, v_s, f_c)))';
    obj.EKF(k).P = (eye(7)-obj.EKF(k).K*obj.EKF(k).C)*obj.EKF(k).P;
    obj.EKF(k).xinxi = cell2mat(doppler_obs_array(k))-fd(p_s, obj.EKF(k).S, v_s, f_c);
end
    df_ds(p_s, obj.EKF(k).S, v_s, f_c);
    obj.DDOP(k).DGDOP = sqrt(trace(inv(G'*G)));
end

